/*
 * DHRobot.java -
 * Copyright (c) 2005 Carmine Lia
 * e-mail:      carmine.lia@libero.it
 * web-address: http://www.intermedia.sa.it/lia

 *  This program is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU General Public License
 *  as published by the Free Software Foundation; either version 2
 *  of the License, or any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
 *
 */
package javax.robotics.engine.robots;

import java.io.File;
import java.io.IOException;
import java.io.InputStream;
import java.net.URI;

import javax.robotics.vecmath.*;
import javax.robotics.engine.JRoboOp;
import javax.robotics.util.RemoteFile;
import javax.robotics.vecmath.Matrix;
import javax.robotics.vecmath.RMatrix4d;
import javax.robotics.vecmath.RVector;
import javax.robotics.vecmath.RVector3d;
import javax.robotics.xml.XmlRobot;

import org.apache.xmlbeans.XmlException;

/**
 * <p>
 * This class implements kinematics and dynamics model of a robot manipulator
 * based on Denavit-Hartenberg table.
 * </p>
 * <p>
 * Wrapper class for C++ library ROBOOP.
 * </p>
 * 
 * @author <a href="http://www.intermedia.sa.it/lia/">Carmine Lia</a>
 * @since 1.0.0
 * @version 30/10/2005
 * 
 */
public class DHRobot extends JRoboOp implements Robot
{
    /**
     * 
     */
    private static final long serialVersionUID = 3256727294520342068L;

    private int dof = 0;
    private int adof = 0;
    
    private short robotID = 0;
    private XmlRobot conf;
    private RVector qMax, qMin, qOffset;

    private void init()
    {
        dof = getDof(robotID);
        adof = getAvDof(robotID);
        qMax = new RVector(getQMax(robotID));
        qMin = new RVector(getQMin(robotID));
        qOffset = new RVector(getQOffset(robotID));
    }
    // ------------- Constructors

    /**
     * Default Robot constructor.
     * 
     */
    public DHRobot()
    {
        robotID = JRoboOp.ID;
        createRobot(robotID);
        init();
    }

    /**
     * Robot constructor with init parameter.
     * <p>
     * <table>
     * <th>Column</th>
     * <th>Variable</th>
     * <th>Description</th>
     * <tr>
     * <td>1</td>
     * <td>&sigma;</td>
     * <td>joint type (revolute=0, prismatic=1)</td>
     * </tr>
     * <tr>
     * <td>2</td>
     * <td>&theta;</td>
     * <td>Denavit-Hartenberg parameter</td>
     * </tr>
     * <tr>
     * <td>3</td>
     * <td>d</td>
     * <td>Denavit-Hartenberg parameter</td>
     * </tr>
     * <tr>
     * <td>4</td>
     * <td>a</td>
     * <td>Denavit-Hartenberg parameter</td>
     * </tr>
     * <tr>
     * <td>5</td>
     * <td>&alpha;</td>
     * <td>Denavit-Hartenberg parameter </tr>
     * <tr>
     * <td>6</td>
     * <td>q<sub>min</sub></td>
     * <td>minimum value of joint variable</td>
     * </tr>
     * <tr>
     * <td>7</td>
     * <td>q<sub>max</sub></td>
     * <td>maximum value of joint variable</td>
     * </tr>
     * <tr>
     * <td>8</td>
     * <td>&theta;<sub>off</sub></td>
     * <td>joint offset</td>
     * </tr>
     * <tr>
     * <td>9</td>
     * <td>m</td>
     * <td>mass of the link</td>
     * </tr>
     * <tr>
     * <td>10</td>
     * <td>c<sub>x</sub></td>
     * <td>center of mass along axis x</td>
     * </tr>
     * <tr>
     * <td>11</td>
     * <td>c<sub>y</sub></td>
     * <td>center of mass along axis y</td>
     * </tr>
     * <tr>
     * <td>12</td>
     * <td>c<sub>z</sub></td>
     * <td>center of mass along axis z</td>
     * </tr>
     * <tr>
     * <td>13</td>
     * <td>I<sub>xx</sub></td>
     * <td>element xx of the inertia tensor matrix</td>
     * </tr>
     * <tr>
     * <td>14</td>
     * <td>I<sub>xy</sub></td>
     * <td>element xy of the inertia tensor matrix</td>
     * </tr>
     * <tr>
     * <td>15</td>
     * <td>I<sub>xz</sub></td>
     * <td>element xz of the inertia tensor matrix</td>
     * </tr>
     * <tr>
     * <td>16</td>
     * <td>I<sub>yy</sub></td>
     * <td>element yy of the inertia tensor matrix</td>
     * </tr>
     * <tr>
     * <td>17</td>
     * <td>I<sub>yz</sub></td>
     * <td>element yz of the inertia tensor matrix</td>
     * </tr>
     * <tr>
     * <td>18</td>
     * <td>I<sub>zz</sub></td>
     * <td>element zz of the inertia tensor matrix</td>
     * </tr>
     * <tr>
     * <td>19</td>
     * <td>I<sub>m</sub></td>
     * <td>motor rotor inertia</td>
     * </tr>
     * <tr>
     * <td>20</td>
     * <td>G<sub>r</sub></td>
     * <td>motor gear ratio</td>
     * </tr>
     * <tr>
     * <td>21</td>
     * <td>B</td>
     * <td>motor viscous friction coefficient</td>
     * </tr>
     * <tr>
     * <td>22</td>
     * <td>C<sub>f</sub></td>
     * <td>motor Coulomb friction coefficient</td>
     * </tr>
     * <tr>
     * <td>23</td>
     * <td><i>immobile</i></td>
     * <td>flag for the kinematics and inverse kinematics (if true joint is
     * locked, if false joint is free)</td>
     * </tr>
     * </table>
     * </p>
     * 
     * @param nLinks
     *            number of robot links
     * @param initRobot
     *            vector of robot parameter; dimension: nLinks x 23
     */
    public DHRobot(int nLinks, double[] initRobot)
    {
        robotID = JRoboOp.ID;
        createRobot(robotID, initRobot, nLinks, INITROBOT_DIM + INITMOTOR_DIM);
        init();

    }

    /**
     * Robot constructor with init parameter and motor parameter.
     * 
     * @param nLinks
     *            number of links or robot
     * @param initRobot
     *            vector of robot parameter; dimension: nLinks x 19
     * @param initMotor
     *            vector of motor robot parameter; dimension: nLinks x 4
     * 
     * @see #DHRobot(int, double[])
     */
    public DHRobot(int nLinks, double[] initRobot, double[] initMotor)
    {
        robotID = JRoboOp.ID;
        createRobot(robotID, initRobot, nLinks, INITROBOT_DIM, initMotor,
                nLinks, INITMOTOR_DIM);
        init();

    }
    /**
     * Robot constructor with ROBOOP file parameter.
     * 
     * @param fileName
     *            configuration file name of robot parameter
     * @param robotName
     *            name of the robot
     * @deprecated
     */
    public DHRobot(String fileName, String robotName)
    {
        robotID = JRoboOp.ID;
        createRobot(robotID, fileName, robotName);
        init();

    }
    /**
     * Robot constructor with URI of xml file parameter. The URI can be an URL
     * or a file path.
     * 
     * @param uri
     *            xml configuration file URI of robot parameter
     * @throws XmlException
     * @throws IOException
     */
    public DHRobot(URI uri) throws XmlException, IOException
    {
        File fileName = null;
        if(uri.getScheme().equals("http"))
        {
            RemoteFile rf = new RemoteFile(uri.toURL());
            fileName = rf.getFile();
        }
        else
            fileName = new File(uri);

        int nLinks;
        robotID = JRoboOp.ID;
        conf = new XmlRobot(fileName);
        nLinks = conf.getNLinks();
        createRobot(robotID, conf.getLinkData(), nLinks, INITROBOT_DIM, conf
                .getMotorData(), nLinks, INITMOTOR_DIM);
        init();

    }
    /**
     * Robot constructor with xml file parameter.
     * 
     * @param fileName
     *            xml configuration file name of robot parameter
     * @throws XmlException
     * @throws IOException
     */
    public DHRobot(String fileName) throws XmlException, IOException
    {
        this(new File(fileName));

    }
    /**
     * Robot constructor with file parameter.
     * 
     * @param fileName
     *            xml configuration file name of robot parameter
     * @throws XmlException
     * @throws IOException
     */
    public DHRobot(File fileName) throws XmlException, IOException
    {
        int nLinks;
        robotID = JRoboOp.ID;
        conf = new XmlRobot(fileName);
        nLinks = conf.getNLinks();
        createRobot(robotID, conf.getLinkData(), nLinks, INITROBOT_DIM, conf
                .getMotorData(), nLinks, INITMOTOR_DIM);
        init();

    }
    
    /**
     * Robot constructor with xml file parameter as input stream.
     * 
     * @param fileName
     *            xml configuration file name of robot parameter
     * @throws XmlException
     * @throws IOException
     */
    public DHRobot(InputStream fileName) throws XmlException, IOException
    {
        int nLinks;
        robotID = JRoboOp.ID;
        conf = new XmlRobot(fileName);
        nLinks = conf.getNLinks();
        createRobot(robotID, conf.getLinkData(), nLinks, INITROBOT_DIM, conf
                .getMotorData(), nLinks, INITMOTOR_DIM);
        init();

    }
    // ------------- Public methods

    /**
     * Gets Robot class ID
     * 
     * @return ID
     */
    public final short getID()
    {
        return robotID;
    }

    /*
     * Gets converge flag.
     * 
     * @see #invKine(RMatrix4d, int, int) @return converge flag
     * 
     * public boolean getConverge() { return getConverge(robotID); }
     * 
     * 
     * Gets error flag.
     * 
     * @return error flag.
     * 
     * public short getError() { return getError(robotID); }
     */
    /*
     * Robot Variables
     */

    public final XmlRobot getRobotConfiguration()
    {
        return conf;
    }
    /*
     * (non-Javadoc)
     * 
     * @see javax.robotics.engine.robots.Robot#getDof()
     */
    public final int getDof()
    {
        return dof; // getDof(robotID);
    }
    
    /* (non-Javadoc)
     * @see javax.robotics.engine.robots.Robot#getAvailableDof()
     */
    public final int getAvailableDof()
    {
        return adof; 
    }

    /*
     * Getters for joint variables
     */

    public final RVector getQMin()
    {
        return qMin;
    }

    public final RVector getQMax()
    {
        return qMax;
    }

    public final RVector getQOffset()
    {
        return qOffset;
    }

    public RVector getQ()
    {
        final RVector q = new RVector(getQ(robotID));
        return q;
    }

    /*
     * (non-Javadoc)
     * 
     * @see javax.robotics.engine.robots.Robot#getQ(int)
     */
    public double getQ(int i)
    {
        return getQ(robotID, i);
    }

    /*
     * (non-Javadoc)
     * 
     * @see javax.robotics.engine.robots.Robot#getQp()
     */
    public RVector getQp()
    {
        final RVector qp = new RVector(getQp(robotID));
        return qp;
    }

    /*
     * (non-Javadoc)
     * 
     * @see javax.robotics.engine.robots.Robot#getQpp()
     */
    public RVector getQpp()
    {
        final RVector qpp = new RVector(getQ(robotID));
        return qpp;
    }

    /*
     * Gets acceleration of i-th joint<br> Not implemented in ROBOOP.
     * 
     * <p> </p>
     * 
     * @param i joint @return join acceleration
     * 
     * public double getQpp(int i) { return getQpp(robotID, i); }
     */
    /*
     * Setters for joint variables
     */
    /*
     * (non-Javadoc)
     * 
     * @see javax.robotics.engine.robots.Robot#setQ(javax.robotics.vecmath.RVector)
     */
    public void setQ(RVector q)
    {
        final double[] q_ = new double[q.getSize()];
        q.get(q_);
        setQ(robotID, q_);
    }

    public void setQ(double[] q)
    {
        setQ(robotID, q);

    }
    /*
     * public void setQ(Object q) { if(q instanceof double[]); if(q instanceof
     * RVector); }
     */
    public void setQ(double q, int i)
    {
        setQ(robotID, q, i);
    }

    public void setQp(RVector qp)
    {
        final double[] qp_ = new double[qp.getSize()];
        qp.get(qp_);
        setQp(robotID, qp_);
    }

    public void setQp(double[] qp)
    {
        setQp(robotID, qp);

    }

    public void setQpp(RVector qpp)
    {
        final double[] qpp_ = new double[qpp.getSize()];
        qpp.get(qpp_);
        setQpp(robotID, qpp_);
    }

    public void setQpp(double[] qpp)
    {
        setQpp(robotID, qpp);

    }

    /*
     * ----------------------------- Robot Kinematics
     */
    public RVector invKine(RMatrix4d Tobj)
    {
        final double[] tobj = new double[HOMOGENEOUS_MATRIX_DIM];
        final RVector q;
        Tobj.get(tobj);
        q = new RVector(invKine(robotID, tobj));
        return q;
    }

    public RVector invKine(RMatrix4d Tobj, int mj, int endlink)
    {
        final RVector q;
        final RVector temp;
        final boolean converge;
        final double[] tobj = new double[HOMOGENEOUS_MATRIX_DIM];
        Tobj.get(tobj);
        temp = new RVector(invKine(robotID, tobj, mj, endlink));
        converge = getConverge(robotID);
        // System.out.println(converge+"\n"+q);
        q = (!converge) ? null : temp;
        return q;
    }

    public Matrix jacobian()
    {
        final Matrix j = new Matrix(JACOBIAN_DIMENSION, dof, jacobian(robotID));
        return j;
    }

    public Matrix jacobian(int ref)
    {
        final Matrix j = new Matrix(JACOBIAN_DIMENSION, dof, jacobian(robotID,
                ref));
        return j;
    }

    public Matrix jacobianDot()
    {
        final Matrix j = new Matrix(JACOBIAN_DIMENSION, dof,
                jacobianDot(robotID));
        return j;
    }

    public Matrix jacobianDot(int ref)
    {
        final Matrix j = new Matrix(JACOBIAN_DIMENSION, dof, jacobianDot(
                robotID, ref));
        return j;
    }

    public Matrix jacobianDLSinv(double eps, double lambdaMax)
    {
        final Matrix j = new Matrix(JACOBIAN_DIMENSION, JACOBIAN_DIMENSION,
                jacobianDLSinv(robotID, eps, lambdaMax));
        return j;
    }

    public RMatrix4d kine()
    {
        final RMatrix4d T = new RMatrix4d(kine(robotID));
        return T;
    }

    public RMatrix4d kine(int j)
    {
        final RMatrix4d T = new RMatrix4d(kine(robotID, j));
        return T;
    }

    public RMatrix4d dTdqi(int j)
    {
        final RMatrix4d T = new RMatrix4d(dTdqi(robotID, j));
        return T;
    }

    /*
     * --------------------------------------------- Robot Dynamics
     */
    public RVector acceleration(RVector q, RVector qp, RVector tau)
    {
        final int dof_loc = q.getSize();
        final double[] q_ = new double[dof_loc];
        final double[] qp_ = new double[dof_loc];
        final double[] tau_ = new double[dof_loc];
        final RVector qpp;
        q.get(q_);
        qp.get(qp_);
        tau.get(tau_);
        qpp = new RVector(acceleration(robotID, q_, qp_, tau_));
        return qpp;
    }

    public RVector acceleration(double[] q, double[] qp, double[] tau)
    {
        final RVector qpp = new RVector(acceleration(robotID, q, qp, tau));
        return qpp;
    }

    public RVector acceleration(RVector q, RVector qp, RVector tauCmd,
            RVector3d Fext, RVector3d Next)
    {
        final int dof_loc = q.getSize();
        final double[] q_ = new double[dof_loc];
        final double[] qp_ = new double[dof_loc];
        final double[] tauCmd_ = new double[dof_loc];
        final double[] Fext_ = new double[SPACE_DIM];
        final double[] Next_ = new double[SPACE_DIM];
        final RVector qpp;
        q.get(q_);
        qp.get(qp_);
        tauCmd.get(tauCmd_);
        Fext.get(Fext_);
        Next.get(Next_);
        qpp = new RVector(acceleration(robotID, q_, qp_, tauCmd_, Fext_, Next_));
        return qpp;
    }

    public RVector acceleration(double[] q, double[] qp, double[] tauCmd,
            double[] Fext, double[] Next)
    {
        final RVector qpp = new RVector(acceleration(robotID, q, qp, tauCmd,
                Fext, Next));
        return qpp;
    }

    public RVector torque(RVector q, RVector qp, RVector qpp)
    {
        final int dof_loc = q.getSize();
        final double[] q_ = new double[dof_loc];
        final double[] qp_ = new double[dof_loc];
        final double[] qpp_ = new double[dof_loc];
        final RVector tau;
        q.get(q_);
        qp.get(qp_);
        qpp.get(qpp_);
        tau = new RVector(torque(robotID, q_, qp_, qpp_));
        return tau;
    }

    public RVector torque(double[] q, double[] qp, double[] qpp)
    {
        final RVector tau = new RVector(torque(robotID, q, qp, qpp));
        return tau;
    }

    public RVector torque(RVector q, RVector qp, RVector qpp, RVector3d Fext,
            RVector3d Next)
    {
        final int dof_loc = q.getSize();
        final double[] q_ = new double[dof_loc];
        final double[] qp_ = new double[dof_loc];
        final double[] qpp_ = new double[dof_loc];
        final double[] Fext_ = new double[SPACE_DIM];
        final double[] Next_ = new double[SPACE_DIM];
        final RVector tau;
        q.get(q_);
        qp.get(qp_);
        qpp.get(qpp_);
        Fext.get(Fext_);
        Next.get(Next_);
        tau = new RVector(torque(robotID, q_, qp_, qpp_, Fext_, Next_));
        return tau;
    }

    public RVector torque(double[] q, double[] qp, double[] qpp, double[] Fext,
            double[] Next)
    {
        final RVector tau = new RVector(torque(robotID, q, qp, qpp, Fext, Next));
        return tau;
    }

    public RVector torqueGravity()
    {
        final RVector tau = new RVector(torqueGravity(robotID));
        return tau;
    }

    public RVector torqueC(RVector qp)
    {
        final double[] qp_ = new double[qp.getSize()];
        final RVector tau;
        qp.get(qp_);
        tau = new RVector(torqueCoriolis(robotID, qp_));
        return tau;
    }

    public RVector torqueViscousFriction(RVector qp)
    {
        final double[] qp_ = new double[qp.getSize()];
        final RVector tau;
        qp.get(qp_);
        tau = new RVector(torqueViscousFriction(robotID, qp_));
        return tau;
    }

    public RVector torqueCoulombFriction(RVector qp)
    {
        final double[] qp_ = new double[qp.getSize()];
        final RVector tau;
        qp.get(qp_);
        tau = new RVector(torqueCoulombFriction(robotID, qp_));
        return tau;
    }

    public Matrix inertia(RVector q)
    {
        final int n = q.getSize();
        final double[] q_ = new double[n];
        final Matrix b;
        q.get(q_);
        b = new Matrix(n, inertia(robotID, q_));
        return b;
    }

    public RVector3d getGravity()
    {
        return new RVector3d(getGravity(robotID));
    }

    public void setGravity(RVector3d g)
    {
        final double[] g_ = new double[SPACE_DIM];
        g_[0] = g.x;
        g_[1] = g.y;
        g_[2] = g.z;
        setGravity(robotID, g_);
    }
}
